#对x进行滤波
def kalman_filter(x_measurements, Q, R, P_initial):
    x_estimated = [x_measurements[0]]  # 初始状态估计为第一个测量值
    P = P_initial  # 初始估计协方差
    estimates = [x_estimated[0]]  # 存储估计值

    # 处理后续测量值
    for z in x_measurements[1:]:
        # 预测
        x_pred = x_estimated[-1]  # 预测状态
        P_pred = P + Q  # 预测协方差

        # 更新
        K = P_pred / (P_pred + R)  # 卡尔曼增益
        x_update = x_pred + K * (z - x_pred)  # 更新状态
        P = (1 - K) * P_pred  # 更新协方差

        # 存储结果
        x_estimated.append(x_update)
        estimates.append(x_update)

    return estimates

# 输入数据
x_measurements = [10, 30,40]  # 已知的 x 值
Q = 0.0001  # 过程噪声协方差
R = 0.0001  # 观测噪声协方差
P_initial = 1  # 初始估计协方差

# 应用卡尔曼滤波
estimates = kalman_filter(x_measurements, Q, R, P_initial)

# 输出结果
print("测量值:", x_measurements)
print("估计值:", [round(e, 4) for e in estimates])